/****************************************************************************
 *
 *  Heterogeneous 10PWM passthrough module (systemcmds copy)
 *
 *  This file is a copy of src/modules/hetero_passthrough/hetero_passthrough.cpp
 *  placed under src/systemcmds so the binary is packaged into ROMFS /bin for
 *  quick testing. Behavior and safety checks are identical to the module
 *  version: it subscribes to manual_control_setpoint and publishes actuator_motors
 *  when the vehicle is armed.
 *
 *  版本 2.3.0 - 优化yaw控制，更符合人体工程学，操纵更丝滑：
 *  1. 采用四旋翼标准控制逻辑，自旋控制更自然
 *  2. 优化参数配置，响应更迅速
 *  3. 改进状态机逻辑，模式切换更平滑
 *  4. 彻底解决遥控器回中时机器被拉回来的问题
 */

#include <px4_platform_common/module.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/module_params.h>

#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_local_position.h>

#include <lib/mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <px4_arch/io_timer.h>
#include <parameters/param.h>

#include "Control.hpp"
#include "vector_angles.hpp"

using namespace time_literals;

class HeteroPassthrough : public ModuleBase<HeteroPassthrough>, public px4::ScheduledWorkItem
{
public:
    HeteroPassthrough() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) {}

    virtual ~HeteroPassthrough() { ScheduleClear(); }

    static int task_spawn(int argc, char *argv[])
    {
        HeteroPassthrough *instance = new HeteroPassthrough();

        if (!instance) {
            PX4_ERR("alloc failed");
            return PX4_ERROR;
        }

        _object.store(instance);

        if (instance->init()) {
            return PX4_OK;
        }

        delete instance;
        _object.store(nullptr);
        return PX4_ERROR;
    }

    bool init()
    {
        // schedule at 50 Hz
        ScheduleOnInterval(20_ms);
        return true;
    }

    void Run() override
    {
        if (should_exit()) {
            ScheduleClear();
            return;
        }

        // 更新my_control参数
        param_get(param_find("MY_YAW_KP"), &_my_control_yaw_kp);
        param_get(param_find("MY_YAW_KD"), &_my_control_yaw_kd);
        param_get(param_find("MY_YAW_KI"), &_my_control_yaw_ki);
        param_get(param_find("MY_YAW_INT_LIM"), &_my_control_yaw_integral_limit);
        param_get(param_find("MY_YAW_OUT_LIM"), &_my_control_yaw_output_limit);

        // Yaw速度环参数
        param_get(param_find("MY_YAW_SP_KP"), &_my_control_yaw_speed_kp);
        param_get(param_find("MY_YAW_SP_KI"), &_my_control_yaw_speed_ki);
        param_get(param_find("MY_YAW_SP_ILIM"), &_my_control_yaw_speed_integral_limit);

        param_get(param_find("MY_ALT_POS_KP"), &_my_control_alt_kp_pos);
        param_get(param_find("MY_ALT_POS_KI"), &_my_control_alt_ki_pos);
        param_get(param_find("MY_ALT_POS_KD"), &_my_control_alt_kd_pos);
        param_get(param_find("MY_ALT_VEL_KP"), &_my_control_alt_kp_vel);
        param_get(param_find("MY_ALT_VEL_KI"), &_my_control_alt_ki_vel);
        param_get(param_find("MY_ALT_VEL_KD"), &_my_control_alt_kd_vel);

        param_get(param_find("MY_FILTER_ALPHA"), &_my_control_filter_alpha);
        param_get(param_find("MY_BLEND_RATE"), &_my_control_blend_rate);

        // 水平位置控制参数
        param_get(param_find("MY_POS_X_KP"), &_my_control_pos_x_kp);
        param_get(param_find("MY_POS_X_KI"), &_my_control_pos_x_ki);
        param_get(param_find("MY_POS_X_KD"), &_my_control_pos_x_kd);
        param_get(param_find("MY_POS_X_ILIM"), &_my_control_pos_x_ilim);
        param_get(param_find("MY_POS_X_OLIM"), &_my_control_pos_x_olim);

        param_get(param_find("MY_POS_Y_KP"), &_my_control_pos_y_kp);
        param_get(param_find("MY_POS_Y_KI"), &_my_control_pos_y_ki);
        param_get(param_find("MY_POS_Y_KD"), &_my_control_pos_y_kd);
        param_get(param_find("MY_POS_Y_ILIM"), &_my_control_pos_y_ilim);
        param_get(param_find("MY_POS_Y_OLIM"), &_my_control_pos_y_olim);

        // 水平速度控制参数
        param_get(param_find("MY_VEL_X_KP"), &_my_control_vel_x_kp);
        param_get(param_find("MY_VEL_X_KI"), &_my_control_vel_x_ki);
        param_get(param_find("MY_VEL_X_KD"), &_my_control_vel_x_kd);
        param_get(param_find("MY_VEL_X_ILIM"), &_my_control_vel_x_ilim);
        param_get(param_find("MY_VEL_X_OLIM"), &_my_control_vel_x_olim);

        param_get(param_find("MY_VEL_Y_KP"), &_my_control_vel_y_kp);
        param_get(param_find("MY_VEL_Y_KI"), &_my_control_vel_y_ki);
        param_get(param_find("MY_VEL_Y_KD"), &_my_control_vel_y_kd);
        param_get(param_find("MY_VEL_Y_ILIM"), &_my_control_vel_y_ilim);
        param_get(param_find("MY_VEL_Y_OLIM"), &_my_control_vel_y_olim);

        // 位置保持参数
        param_get(param_find("MY_POS_HOLD_DZ"), &_my_control_pos_hold_deadzone);


        // check manual control
        // use copy() to always get the latest sample (even if not "new" since
        // last Run()). This ensures we re-publish zeros when sticks return to
        // center instead of early-returning and leaving previous actuator values
        // active downstream.
        manual_control_setpoint_s manual{};
        _manual_sub.copy(&manual);

        // check armed status
        vehicle_status_s vstatus{};
        _vehicle_status_sub.copy(&vstatus);

        // 检查解锁状态并设置初始姿态
        static bool was_armed_last_cycle = false;

        // 检测从锁定到解锁的转换，直接设置初始姿态和高度
        if (vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED && !was_armed_last_cycle) {
            was_armed_last_cycle = true;
          //  PX4_INFO("🔓 检测到解锁，即将设置姿态保持初始位置和高度");

            // 立即获取当前角度作为姿态保持初始目标
            vehicle_attitude_s attitude_temp{};
            if (_vehicle_attitude_sub.copy(&attitude_temp)) {
                matrix::Eulerf euler_temp(matrix::Quatf(attitude_temp.q));
                float current_yaw_temp = euler_temp.psi();
                _last_yaw = current_yaw_temp;  // 直接设置为当前角度
             //   PX4_INFO("✅ 姿态保持初始位置已设置: %.2f度", (double)(_last_yaw * 57.3f));
            }

            // 立即获取当前高度作为高度保持初始目标
            vehicle_local_position_s local_position_temp{};
            if (_vehicle_local_position_sub.copy(&local_position_temp) &&
                local_position_temp.z_valid && PX4_ISFINITE(local_position_temp.z)) {
                _target_altitude = local_position_temp.z;
               // PX4_INFO("✅ 高度保持初始位置已设置: %.2f米", (double)_target_altitude);

                // 设置水平位置保持初始目标
                if (local_position_temp.xy_valid && PX4_ISFINITE(local_position_temp.x) && PX4_ISFINITE(local_position_temp.y)) {
                    _target_position_x = local_position_temp.x;
                    _target_position_y = local_position_temp.y;
                    //PX4_INFO("✅ 水平位置保持初始位置已设置: X=%.2f米, Y=%.2f米",
                            //(double)_target_position_x, (double)_target_position_y);
                } else {
                    PX4_WARN("⚠️ 无法获取有效水平位置数据，水平位置保持未设置");
                }
            } else {
                PX4_WARN("⚠️ 无法获取有效高度数据，高度保持未设置");
            }
        } else if (vstatus.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
            was_armed_last_cycle = false;
            return;  // 未解锁时不运行控制逻辑
        }

        actuator_motors_s motors{};
        motors.timestamp = hrt_absolute_time();
        motors.timestamp_sample = manual.timestamp_sample;

        // initialize as 0 to ensure unused channels are explicitly zeroed
        // (avoids leaving stale/NaN values that downstream mixers/drivers
        // might interpret unexpectedly)
        for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) {
            motors.control[i] = 0.0f;
        }

        const float deadzone = 0.05f;
        // small absolute threshold to force values very close to zero to 0.0f
        // helps avoid low-amplitude jitter causing motors to spin slowly.
        const float zero_threshold = 0.02f;

        // 添加输入平滑滤波 - 减少遥控器输入的抖动
        // 版本2.1.0优化：添加了遥控器输入的平滑滤波，减少抖动
        static float last_throttle = 0.0f;
        static float last_yaw = 0.0f;
        static float last_pitch = 0.0f;
        static float last_roll = 0.0f;

        // 使用参数化的滤波系数进行一阶低通滤波
        float throttle = last_throttle * (1.0f - _my_control_filter_alpha) + manual.throttle * _my_control_filter_alpha;
        float yaw = last_yaw * (1.0f - _my_control_filter_alpha) + manual.yaw * _my_control_filter_alpha;
        float pitch = last_pitch * (1.0f - _my_control_filter_alpha) + manual.pitch * _my_control_filter_alpha;
        float roll = last_roll * (1.0f - _my_control_filter_alpha) + manual.roll * _my_control_filter_alpha;

        last_throttle = throttle;
        last_yaw = yaw;
        last_pitch = pitch;
        last_roll = roll;

        // Yaw控制实现 - 基于多旋翼标准控制逻辑
        // 版本2.1.1优化：修复遥控器回中时机器被拉回来的问题
        // 使用角速度控制而不是角度控制，实现平滑过渡
        float yaw_control = 0.0f;
        const float yaw_deadzone = 0.2f; // 死区阈值

        // 获取当前时间用于PID计算
        hrt_abstime current_time = hrt_absolute_time();
        float dt = 0.02f; // 默认时间步长（50Hz）

        // 计算时间差
        if (_last_time > 0) {
            dt = (current_time - _last_time) * 1e-6f; // 转换为秒
            dt = math::constrain(dt, 0.001f, 0.1f);  // 限制时间步长范围
        }
        _last_time = current_time;

        // 读取姿态和角速度数据
        vehicle_attitude_s attitude{};
        vehicle_angular_velocity_s angular_velocity{};

        bool has_attitude = _vehicle_attitude_sub.copy(&attitude);
        bool has_angular_velocity = _vehicle_angular_velocity_sub.copy(&angular_velocity);
        bool has_valid_sensors = has_attitude && has_angular_velocity && dt > 0.001f;

        // 检查遥控器输入状态
        bool has_manual_yaw_input = (fabsf(yaw) > yaw_deadzone);

        // 多旋翼标准yaw控制：遥控器输入映射到角速度
        // 当遥控器回中时，期望角速度为零，自然减速停止
        // 避免切换到姿态保持模式时产生反向扭矩
        const float max_yaw_rate = 0.01f; // 最大yaw角速度 (rad/s)
        float yaw_rate_setpoint = 0.0f;

        if (has_manual_yaw_input) {
            // 手动控制：遥控器输入直接映射到角速度
            yaw_rate_setpoint = yaw * max_yaw_rate;

            target_setpoint_yaw += (double)(yaw * max_yaw_rate);
            if (target_setpoint_yaw > M_PI) {
                target_setpoint_yaw -= 2.0 * M_PI;
            } else if (target_setpoint_yaw < -M_PI) {
                target_setpoint_yaw += 2.0 * M_PI;
            }

        } else {
            // 遥控器回中：期望角速度为零，自然减速
            yaw_rate_setpoint = 0.0f;
        }

        // 高度PID控制实现
        float altitude_control = 0.0f;
        const float altitude_deadzone = 0.1f; // 高度控制死区阈值

        // 读取位置数据
        vehicle_local_position_s local_position{};
        bool has_local_position = _vehicle_local_position_sub.copy(&local_position);
        bool has_valid_altitude = has_local_position && local_position.z_valid && PX4_ISFINITE(local_position.z);

        // 检查高度控制模式切换
        bool has_manual_altitude_input = (fabsf(throttle) > altitude_deadzone);
        static bool was_manual_altitude_last_cycle = false;

        // 检测高度模式切换
        if (has_manual_altitude_input && !was_manual_altitude_last_cycle) {
            // 从高度保持切换到手动控制，重置积分项
            _altitude_integral = 0.0f;
           // PX4_INFO("🔄 切换到手动高度控制，积分项已重置");
        }
        was_manual_altitude_last_cycle = has_manual_altitude_input;

        // 高度控制平滑混合
        static float altitude_control_blend = 1.0f;  // 高度控制混合系数 (0=手动, 1=高度保持)
        const float altitude_blend_rate = 0.05f;

        if (has_manual_altitude_input) {
            altitude_control_blend = math::max(altitude_control_blend - altitude_blend_rate, 0.0f);
        } else {
            altitude_control_blend = math::min(altitude_control_blend + altitude_blend_rate, 1.0f);
        }

        float smooth_altitude_blend = altitude_control_blend * altitude_control_blend * (3.0f - 2.0f * altitude_control_blend);

        // 高度PID控制核心逻辑
        float manual_altitude_control = 0.0f;
        float altitude_hold_control = 0.0f;

        if (has_manual_altitude_input) {
            // 手动高度控制模式 - 反转遥控器输入方向
            manual_altitude_control = -throttle;

            // 手动控制时更新高度保持目标
            if (has_valid_altitude) {
                if (fabsf(_target_altitude) > 0.001f) {
                    _target_altitude = _target_altitude * 0.9f + local_position.z * 0.1f;
                } else {
                    _target_altitude = local_position.z;
                }
            }

            // 积分器保护
            _altitude_integral *= 0.98f;

        } else if (has_valid_altitude) {
            // 高度保持模式：级联PID控制（位置环+速度环）
            // 设置高度保持目标
            if (fabsf(_target_altitude) < 0.001f) {
                _target_altitude = local_position.z;
              //  PX4_INFO("高度保持启动: 目标高度=%.2f米", (double)_target_altitude);
            }

            // ===========================================
            // 位置环（外环）：计算期望垂直速度
            // ===========================================

            // 计算高度误差
            float altitude_error = _target_altitude - local_position.z;

            // 位置环PID参数
            const float kp_pos = -5.0f;          // 位置环比例增益（正号，与遥控器方向一致）
            const float ki_pos = -0.05f;         // 位置环积分增益（正号，与遥控器方向一致）
            const float kd_pos = -0.1f;          // 位置环微分增益（正号，与遥控器方向一致）
            const float pos_integral_limit = 0.3f;   // 位置环积分限幅
            const float pos_output_limit = 1.0f;     // 位置环输出限幅（期望速度范围）

            // 位置环比例项
            float p_term_pos = kp_pos * altitude_error;

            // 位置环积分项 - 带抗饱和处理
            _altitude_integral += ki_pos * altitude_error * dt;
            _altitude_integral = math::constrain(_altitude_integral, -pos_integral_limit, pos_integral_limit);
            float i_term_pos = _altitude_integral;

            // 位置环微分项 - 垂直速度阻尼
            float current_vertical_velocity = local_position.vz;
            float d_term_pos = kd_pos * current_vertical_velocity;  // 添加负号，因为速度阻尼应该抑制运动

            // 位置环输出：期望垂直速度
            _target_velocity = p_term_pos + i_term_pos + d_term_pos;
            _target_velocity = math::constrain(_target_velocity, -pos_output_limit, pos_output_limit);

            // ===========================================
            // 速度环（内环）：计算控制输出
            // ===========================================

            // 计算速度误差
            float velocity_error = _target_velocity - current_vertical_velocity;

            // 速度环PID参数
            const float kp_vel = -0.5f;          // 速度环比例增益（正号，与遥控器方向一致）
            const float ki_vel = -0.0f;          // 速度环积分增益（正号，与遥控器方向一致）
            const float kd_vel = -0.0f;          // 速度环微分增益（正号，与遥控器方向一致）
            const float vel_integral_limit = 0.5f;   // 速度环积分限幅
            const float vel_output_limit = 0.8f;     // 速度环输出限幅

            // 速度环比例项
            float p_term_vel = kp_vel * velocity_error;

            // 速度环积分项 - 带抗饱和处理
            _velocity_integral += ki_vel * velocity_error * dt;
            _velocity_integral = math::constrain(_velocity_integral, -vel_integral_limit, vel_integral_limit);
            float i_term_vel = _velocity_integral;

            // 速度环微分项 - 加速度阻尼（使用速度变化率）
            static float last_velocity = 0.0f;
            float acceleration = (current_vertical_velocity - last_velocity) / dt;
            float d_term_vel = kd_vel * acceleration;  // 添加负号，因为加速度阻尼应该抑制运动
            last_velocity = current_vertical_velocity;

            // 速度环输出：最终控制量
            altitude_hold_control = p_term_vel + i_term_vel + d_term_vel;
            altitude_hold_control = math::constrain(altitude_hold_control, -vel_output_limit, vel_output_limit);

            // 调试信息
            // static int debug_alt_count = 0;
            // if (++debug_alt_count % 50 == 0) {
            //     PX4_INFO("级联控制: 高度误差%.3fm → 期望速度%.3fm/s → 速度误差%.3fm/s → 输出%.3f",
            //             (double)altitude_error, (double)_target_velocity,
            //             (double)velocity_error, (double)altitude_hold_control);
            // }
        }

        // 使用平滑过渡函数混合高度控制输出
        altitude_control = manual_altitude_control * (1.0f - smooth_altitude_blend) + altitude_hold_control * smooth_altitude_blend;



        // Yaw PID控制实现
        // 版本2.2.1：重新引入PID控制，使用参数化的PID增益
        float yaw_control_output = 0.0f;

        if (has_valid_sensors) {
            // 获取当前姿态和角速度
            matrix::Eulerf euler_angles(matrix::Quatf(attitude.q));
            float current_yaw = euler_angles.psi();
            float current_yaw_rate = angular_velocity.xyz[2];

            // 参考多旋翼StickYaw原理：平滑过渡和yaw锁定
            static float yaw_lock_angle = current_yaw;
            static bool yaw_locked = false;

            if (has_manual_yaw_input) {
                // 手动控制模式：使用角速度PID控制
                // 计算角速度误差
                yaw_rate_error = yaw_rate_setpoint - current_yaw_rate;

                yaw_rate_error = (float)target_setpoint_yaw - current_yaw;
                if(yaw_rate_error>3.0f)
                {
                    yaw_rate_error = -0.5;
                }
                else if(yaw_rate_error<-3.0f)
                {
                    yaw_rate_error = 0.5;
                }




                // 角速度PID控制
                // 比例项
                float p_term = _my_control_yaw_speed_kp * yaw_rate_error;

                // 积分项 - 带抗饱和处理
                speed_i += _my_control_yaw_speed_ki * yaw_rate_error * dt;
                speed_i = math::constrain(speed_i, -_my_control_yaw_speed_integral_limit, _my_control_yaw_speed_integral_limit);

                // 微分项 - 角加速度
                float yaw_acceleration = (current_yaw_rate - _last_yaw_rate) / dt;
                float d_term_1 = _my_control_yaw_kd * yaw_acceleration;
                d_term_1 = _my_control_yaw_kd * (-current_yaw_rate);


                // 角速度环输出
                yaw_control_output = p_term + speed_i + d_term_1;

                PX4_INFO("target_setpoint_yaw: %.3f ,current_yaw: %.3f, yaw_rate_error: %.3f ,yaw_control_output: %.3f",
                        (double)target_setpoint_yaw, (double)current_yaw, (double)yaw_rate_error, (double)yaw_control_output);

                //  PX4_INFO("target: %.3f,   current: %.3f,    error: %.3f   ,output: %.3f",
                //         (double)yaw_rate_setpoint, (double)current_yaw_rate, (double)yaw_rate_error, (double)yaw_control_output);


                // 更新yaw锁定角度（平滑更新）
                yaw_lock_angle = yaw_lock_angle * 0.95f + current_yaw * 0.05f;

                // 标记为未锁定状态
                yaw_locked = false;

                // 重置角度环积分项
                _yaw_integral = 0.0f;

                // 保存当前角速度用于下次微分计算
                _last_yaw_rate = current_yaw_rate;

                //PX4_INFO( "1111111: %.3f",
                 //       (double)yaw_control_output);

            } else {
                // 遥控器回中：切换到角度PID控制
                // 检查是否应该切换到yaw锁定
                bool should_lock_yaw = fabsf(current_yaw_rate) < 0.3f; // 角速度小于0.6 rad/s
                // PX4_INFO("yaw_rate: %.3f",
                //         (double)current_yaw_rate);

                if (!yaw_locked && should_lock_yaw) {
                    // 切换到yaw锁定状态
                    yaw_locked = true;
                    yaw_lock_angle = current_yaw; // 锁定到当前位置
                    target_setpoint_yaw = yaw_lock_angle;//将摇杆的值设为锁定角度
                }

                if (yaw_locked) {
                    // yaw锁定模式：使用角度PID维持当前角度
                    float yaw_error = matrix::wrap_pi(yaw_lock_angle - current_yaw);
                    PX4_INFO(" yaw_lock_angle: %.3f,current_yaw: %.3f,yaw_error: %.3f",(double)yaw_lock_angle,(double)current_yaw,(double)yaw_error);
                    // 角度PID控制
                    // 比例项
                    float p_term = _my_control_yaw_kp * yaw_error;

                    // 积分项 - 带抗饱和处理
                    _yaw_integral += _my_control_yaw_ki * yaw_error * dt;
                    _yaw_integral = math::constrain(_yaw_integral, -_my_control_yaw_integral_limit, _my_control_yaw_integral_limit);

                    // 微分项 - 角速度阻尼
                    float d_term = _my_control_yaw_kd * (-current_yaw_rate); // 负号表示阻尼

                    // 角度环输出
                    yaw_control_output = p_term + _yaw_integral + d_term;
                    // PX4_INFO( "2222222: %.3f",
                    //     (double)yaw_control_output);

                } else {
                    // 自然减速模式：使用角速度阻尼控制
                    // 这是修复问题的关键：不产生反向扭矩，只提供阻尼
                    float damping_gain = 0.3f;
                    yaw_control_output = damping_gain * current_yaw_rate;

                    // 逐渐更新锁定角度
                    yaw_lock_angle = yaw_lock_angle * 0.98f + current_yaw * 0.02f;

                    // 逐渐减小积分项
                    _yaw_integral *= 0.95f;
                    speed_i *= 0.95f;
                    // PX4_INFO( "33333: %.3f",
                    //     (double)yaw_control_output);
                }


                //  PX4_INFO("Yaw: %.3f, Yaw Rate: %.3f",
                //        (double)current_yaw, (double)current_yaw_rate);

            }

            // 更新_last_yaw用于兼容现有逻辑
            _last_yaw = yaw_lock_angle;

            // 保存当前角速度用于下次微分计算
            _last_yaw_rate = current_yaw_rate;

            // 限制输出范围
            yaw_control_output = math::constrain(yaw_control_output, -_my_control_yaw_output_limit, _my_control_yaw_output_limit);

        } else {
            // 无传感器时，直接使用遥控器输入
            yaw_control_output = yaw * _my_control_yaw_output_limit;
        }

        // 最终yaw控制输出
        yaw_control = yaw_control_output;


        // ===========================================
        // 水平位置和速度环控制
        // ===========================================
        float pos_x_control = 0.0f;
        float pos_y_control = 0.0f;


        // 检查水平位置控制模式切换
        bool has_manual_pos_x_input = (fabsf(pitch) > _my_control_pos_hold_deadzone);
        bool has_manual_pos_y_input = (fabsf(roll) > _my_control_pos_hold_deadzone);
        bool has_manual_pos_input = has_manual_pos_x_input || has_manual_pos_y_input;
        //bool has_manual_pos_input = false;

        static bool was_manual_pos_last_cycle = false;
        static float pos_control_blend_x = 1.0f;  // X方向控制混合系数 (0=手动, 1=位置保持)
        static float pos_control_blend_y = 1.0f;  // Y方向控制混合系数 (0=手动, 1=位置保持)

        // 检测模式切换
        if (has_manual_pos_input && !was_manual_pos_last_cycle) {
            // 从位置保持切换到手动控制，重置积分项
            _pos_x_integral = 0.0f;
            _pos_y_integral = 0.0f;
            _vel_x_integral = 0.0f;
            _vel_y_integral = 0.0f;
        }
        was_manual_pos_last_cycle = has_manual_pos_input;

        // 平滑混合控制模式
        const float pos_blend_rate = 0.05f;

        if (has_manual_pos_x_input) {
            pos_control_blend_x = math::max(pos_control_blend_x - pos_blend_rate, 0.0f);
        } else {
            pos_control_blend_x = math::min(pos_control_blend_x + pos_blend_rate, 1.0f);
        }

        if (has_manual_pos_y_input) {
            pos_control_blend_y = math::max(pos_control_blend_y - pos_blend_rate, 0.0f);
        } else {
            pos_control_blend_y = math::min(pos_control_blend_y + pos_blend_rate, 1.0f);
        }

        float smooth_pos_blend_x = pos_control_blend_x * pos_control_blend_x * (3.0f - 2.0f * pos_control_blend_x);
        float smooth_pos_blend_y = pos_control_blend_y * pos_control_blend_y * (3.0f - 2.0f * pos_control_blend_y);

        // 水平位置控制核心逻辑
        float manual_pos_x_control = 0.0f;
        float manual_pos_y_control = 0.0f;
        float pos_hold_x_control = 0.0f;
        float pos_hold_y_control = 0.0f;

        // bool has_valid_horizontal_position = has_local_position && local_position.xy_valid &&
        //                                     PX4_ISFINITE(local_position.x) && PX4_ISFINITE(local_position.y);
        bool has_valid_horizontal_position = 1;

        if (has_manual_pos_input) {
            // 手动控制模式：直接使用遥控器输入
            manual_pos_x_control = pitch;
            manual_pos_y_control = roll;

            // 手动控制时更新位置保持目标
            if (has_valid_horizontal_position) {
                if (fabsf(_target_position_x) > 0.001f) {
                    _target_position_x = _target_position_x * 0.9f + local_position.x * 0.1f;
                } else {
                    _target_position_x = local_position.x;
                }

                if (fabsf(_target_position_y) > 0.001f) {
                    _target_position_y = _target_position_y * 0.9f + local_position.y * 0.1f;
                } else {
                    _target_position_y = local_position.y;
                }
            }

            // 积分器保护
            _pos_x_integral *= 0.98f;
            _pos_y_integral *= 0.98f;
            _vel_x_integral *= 0.98f;
            _vel_y_integral *= 0.98f;

        } else if (has_valid_horizontal_position) {
            // 调试模式：暂时注释掉位置环，只使用速度环
            // 设置位置保持目标（仅用于显示）
            // if (fabsf(_target_position_x) < 0.001f && fabsf(_target_position_y) < 0.001f) {
            //     _target_position_x = local_position.x;
            //     _target_position_y = local_position.y;
            //     PX4_INFO("水平速度环调试模式启动: 当前位置 X=%.2f米, Y=%.2f米",
            //             (double)_target_position_x, (double)_target_position_y);
            // }

            // ===========================================
            // X方向速度环（内环）：直接控制速度
            // ===========================================
            float current_velocity_x = local_position.vx;

            // 调试：暂时将期望速度设为0，测试速度环响应
            float target_velocity_x_debug = 0.0f;
            float velocity_x_error = target_velocity_x_debug - current_velocity_x;

            // 速度环比例项
            float p_term_vel_x = _my_control_vel_x_kp * velocity_x_error;

            // 速度环积分项 - 带抗饱和处理
            _vel_x_integral += _my_control_vel_x_ki * velocity_x_error * dt;
            _vel_x_integral = math::constrain(_vel_x_integral, -_my_control_vel_x_ilim, _my_control_vel_x_ilim);
            float i_term_vel_x = _vel_x_integral;

            // 速度环微分项 - 加速度阻尼
            static float last_velocity_x = 0.0f;
            float acceleration_x = (current_velocity_x - last_velocity_x) / dt;
            float d_term_vel_x = _my_control_vel_x_kd * acceleration_x;
            last_velocity_x = current_velocity_x;

            // 速度环输出：X方向控制力
            pos_hold_x_control = p_term_vel_x + i_term_vel_x + d_term_vel_x;
            pos_hold_x_control = math::constrain(pos_hold_x_control, -_my_control_vel_x_olim, _my_control_vel_x_olim);

            // ===========================================
            // Y方向速度环（内环）：直接控制速度
            // ===========================================
            float current_velocity_y = local_position.vy;

            // 调试：暂时将期望速度设为0，测试速度环响应
            float target_velocity_y_debug = 0.0f;
            float velocity_y_error = target_velocity_y_debug - current_velocity_y;

            // 速度环比例项
            float p_term_vel_y = _my_control_vel_y_kp * velocity_y_error;

            // 速度环积分项 - 带抗饱和处理
            _vel_y_integral += _my_control_vel_y_ki * velocity_y_error * dt;
            _vel_y_integral = math::constrain(_vel_y_integral, -_my_control_vel_y_ilim, _my_control_vel_y_ilim);
            float i_term_vel_y = _vel_y_integral;

            // 速度环微分项 - 加速度阻尼
            static float last_velocity_y = 0.0f;
            float acceleration_y = (current_velocity_y - last_velocity_y) / dt;
            float d_term_vel_y = _my_control_vel_y_kd * acceleration_y;
            last_velocity_y = current_velocity_y;

            // 速度环输出：Y方向控制力
            pos_hold_y_control = p_term_vel_y + i_term_vel_y + d_term_vel_y;
            pos_hold_y_control = math::constrain(pos_hold_y_control, -_my_control_vel_y_olim, _my_control_vel_y_olim);


        }

        // 使用平滑过渡函数混合水平控制输出
        pos_x_control = manual_pos_x_control * (1.0f - smooth_pos_blend_x) + pos_hold_x_control * smooth_pos_blend_x;
        pos_y_control = manual_pos_y_control * (1.0f - smooth_pos_blend_y) + pos_hold_y_control * smooth_pos_blend_y;
         //pos_x_control = manual_pos_x_control * (1.0f - smooth_pos_blend_x) + pos_hold_x_control * smooth_pos_blend_x;
       // pos_y_control = pos_hold_y_control;


        // 🔥 使用控制矩阵转换：将yaw PID输出转换为5678四个电机的力
        // 版本2.1.0优化：使用控制矩阵将yaw PID输出转换为电机控制信号
        // 根据Control.cpp中的控制矩阵，Mz对应yaw力矩，fx和fy对应遥控器的x和y输入
        float u[8] = {0.0f, 0.0f, 0.0f, 0.0f,0.0f, 0.0f, 0.0f, 0.0f}; // [F1, F2, F3, F4] 对应电机5,6,7,8
        ControlCommand cmd;
        cmd.Fx = pos_x_control * 0.99f;   // X方向力，来自水平位置/速度环控制
        cmd.Fy = pos_y_control * 0.99f;  // Y方向力，来自水平位置/速度环控制
        cmd.Mz = yaw_control;  // 偏航力矩，来自yaw PID控制输出
        // 设置期望的力和力矩
        // Fx: 水平x方向力（对应遥控器roll输入）
        // Fy: 水平y方向力（对应遥控器pitch输入）
        // Mz: yaw力矩（对应yaw PID输出）
        //  v_d[0] = pitch * 0.99f;   // 水平x方向力，映射到遥控器roll
        //  v_d[1] =   // 水平y方向力，映射到遥控器pitch
        //  v_d[2] = -yaw_control;    // yaw力矩，使用PID输出，-yaw_control

        // 使用控制矩阵转换
        //control_matrixT(v_d, u);
        eight_thrust_allocate(&cmd, u);

        // 映射到5678四个电机
        // 根据控制矩阵的映射关系：
        // u[0] -> 电机5, u[1] -> 电机6, u[2] -> 电机7, u[3] -> 电机8
        motors.control[2] = math::constrain(u[0], 0.0f, 1.0f);  // 电机5
        motors.control[3] = math::constrain(u[1], 0.0f, 1.0f);  // 电机6
       // motors.control[2] = math::constrain(u[2], 0.0f, 1.0f);  // 电机7
        //motors.control[3] = math::constrain(u[3], 0.0f, 1.0f);  // 电机8        motors.control[2] = math::constrain(u[0], 0.0f, 1.0f);  // 电机5
        motors.control[4] = math::constrain(u[4], 0.0f, 1.0f);  // 电机5，前进
        motors.control[5] = math::constrain(u[5], 0.0f, 1.0f);  // 电机6，后退
        motors.control[7] = math::constrain(u[6], 0.0f, 1.0f);  // 电机7，往左
        motors.control[6] = math::constrain(u[7], 0.0f, 1.0f);  // 电机8，往右


        // ✨ 实时验证控制矩阵输出
        static int debug_count = 0;
        if (++debug_count % 25 == 0) {
            //float abs_error_deg = 0.0f;
            if (!has_manual_yaw_input && has_valid_sensors) {
                matrix::Eulerf euler_angles(matrix::Quatf(attitude.q));
                //float current_yaw = euler_angles.psi();
              //  float yaw_error = matrix::wrap_pi(_last_yaw - current_yaw);
               // abs_error_deg = fabsf(yaw_error * 57.3f);
            }

            // 高度控制调试信息
          //  float altitude_error = 0.0f;
            // if (has_valid_altitude && !has_manual_altitude_input) {
            //     altitude_error = _target_altitude - local_position.z;
            // }

            // if (has_manual_yaw_input) {
            //     PX4_INFO("手动模式: 输入%.2f → 电机5:%.2f 6:%.2f 7:%.2f 8:%.2f",
            //             (double)yaw_control, (double)u[0], (double)u[1], (double)u[2], (double)u[3]);
            // } else if (abs_error_deg > 0.1f) {
            //     PX4_INFO("姿态保持: 误差%.1f° → 电机5:%.2f 6:%.2f 7:%.2f 8:%.2f",
            //             (double)abs_error_deg, (double)u[0], (double)u[1], (double)u[2], (double)u[3]);
            // }

            // 高度控制状态显示
            // if (has_manual_altitude_input) {
            //     PX4_INFO("手动高度: 输入%.2f → 输出%.2f → 电机1:%.2f 2:%.2f 3:%.2f 4:%.2f",
            //             (double)throttle, (double)altitude_control,
            //             (double)motors.control[0], (double)motors.control[1],
            //             (double)motors.control[2], (double)motors.control[3]);
            // } else if (has_valid_altitude && fabsf(altitude_error) > 0.05f) {
            //     PX4_INFO("高度保持: 误差%.2fm → 输出%.2f → 电机1:%.2f 2:%.2f 3:%.2f 4:%.2f",
            //             (double)altitude_error, (double)altitude_control,
            //             (double)motors.control[0], (double)motors.control[1],
            //             (double)motors.control[2], (double)motors.control[3]);
            // }
        }

        // 垂直控制 - 使用高度PID控制输出
        // 高度控制输出为正时：电机3-4转动（上升，阻力上升）
        // 高度控制输出为负时：电机1-2转动（下降，阻力下降）
        if (altitude_control > deadzone) {
            float val = math::constrain(altitude_control, 0.0f, 1.0f);
            motors.control[1] = val; // 电机3
           // motors.control[0] = val; // 电机4

        }
        else if (altitude_control < -deadzone) {
            float val = math::constrain(-altitude_control, 0.0f, 1.0f);
            motors.control[0] = val; // 电机1
           // motors.control[1] = val; // 电机2
        }

        // 🔑 兼容原有手动控制逻辑：新模式下手动输入仍然有效
        // （四电机连续控制已经包含了手动模式的处理）

        // 水平移动控制 - 右摇杆
        // 右摇杆向上（前进）：电机6和8旋转
        // if (pitch > deadzone) {
        //     float val = math::constrain(pitch, 0.0f, 1.0f);
        //     motors.control[5] += val; // 电机6
        //     motors.control[7] += val; // 电机8
        // }
        // // 右摇杆向下（后退）：电机5和7旋转
        // else if (pitch < -deadzone) {
        //     float val = math::constrain(-pitch, 0.0f, 1.0f);
        //     motors.control[4] += val; // 电机5
        //     motors.control[6] += val; // 电机7
        // }

        // // 右摇杆向左（左移）：电机5和8旋转
        // if (roll < -deadzone) {
        //     float val = math::constrain(-roll, 0.0f, 1.0f);
        //     motors.control[4] += val; // 电机5
        //     motors.control[7] += val; // 电机8
        // }
        // // 右摇杆向右（右移）：电机6和7旋转
        // else if (roll > deadzone) {
        //     float val = math::constrain(roll, 0.0f, 1.0f);
        //     motors.control[5] += val; // 电机6
        //     motors.control[6] += val; // 电机7
        // }

        // ===================================
        // 🚀 9号电机 + 13/14号舵机扩展接口
        // ===================================

        // 9号电机：直接与遥控器油门相关（测试用）
        // 提供纯线性映射，便于单独测试
        float motor_9_output = 0.0f;

        // 13号舵机：大舵机，对应original.a
       double servo_11_output = 0.0f;

        // 14号舵机：小舵机，对应original.b
       double servo_12_output = 0.0f;

        // 📊 测试模式配置（参数控制）
        const bool test_mode_enabled = true;  // 测试模式开关
        const float test_response_gain = 0.8f; // 测试响应强度（防止过冲）

        if (test_mode_enabled) {
            // 🔧 使用VectorControl算法计算舵机角度和电机转速
            // 将遥控器输入转换为舵机控制
            // x: pitch (俯仰), y: roll (横滚), z: throttle (油门)
            // 注意：VectorControl函数内部已经处理了z值的正负转换

            // 将遥控器输入[-1,1]映射到VectorControl的输入范围
            // 这里假设VectorControl期望的输入范围是[-100,100]
            int vector_x = static_cast<int>(pitch*1024);   // pitch -> x
            int vector_y = static_cast<int>(roll*1024);    // roll -> y
            int vector_z = static_cast<int>(throttle*1024); // throttle -> z

            // 调用VectorControlWithResult函数计算舵机角度并获取结果
            VectorAngles result = VectorControlWithResult(vector_x, vector_y, vector_z);

            // 将角度结果转换为舵机控制信号
            // original.a (大环角度) -> 13号舵机
            // original.b (小环角度) -> 14号舵机
            // original.output (最大绝对值) -> 9号电机转速

            // 将角度[0,180]度映射到舵机脉冲[500,2500]μs，然后归一化到[0,1]
            // 角度0度 -> 500μs -> 0.0
            // 角度180度 -> 2500μs -> 1.0

            // 13号舵机：大环角度original.a
            servo_11_output = (result.a / 180.0)*2000+500;
            // servo_13_output = math::constrain(servo_13_output, 0.0f, 1.0f);

            // 14号舵机：小环角度original.b
            servo_12_output = (result.b / 180.0)*2000+500;
            // servo_14_output = math::constrain(servo_14_output, 0.0f, 1.0f);

            // 9号电机：使用VectorControl的output作为转速控制
            // original.output是最大绝对值，范围[0,180]，映射到[0,1]
            motor_9_output = static_cast<float>(result.output / 1024) * test_response_gain;
            motor_9_output = math::constrain(motor_9_output, 0.0f, 1.0f);

        }

        // 🔗 映射到扩展执行器通道
        // 9号电机使用actuator_motors通道
        motors.control[8] = motor_9_output;    // 9号电机通道 -> PWM_MAIN_9

        // 使用actuator_motors的13和14号通道作为舵机输出
        // 现在CONFIG_PWM_OUT_COUNT=14，可以使用13和14通道
        // 对应PWM_MAIN_13, PWM_MAIN_14
        // motors.control[10] = 0.8f; // 11号舵机通道 -> PWM_MAIN_11
        // motors.control[10] = servo_13_output;   // 13号舵机 -> PWM_MAIN_13
        // motors.control[11] = servo_14_output;   // 14号舵机 -> PWM_MAIN_14

        //1MHZ，CCR取值0-19999，舵机设定范围500-2500
        // io_timer_set_ccr(10, 1200);
        // io_timer_set_ccr(11, 2000);

        // 约束所有电机输出在[0,1]范围内（包含扩展通道）
        for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) {
            motors.control[i] = math::constrain(motors.control[i], 0.0f, 1.0f);
        }

        // clamp small values to zero to avoid micro-output
        for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) {
            if (fabsf(motors.control[i]) < zero_threshold) {
                motors.control[i] = 0.0f;
            }
        }

        _motors_pub.publish(motors);
        io_timer_set_ccr(12, (int)servo_11_output);
        io_timer_set_ccr(13, (int)servo_12_output);//

      //  io_timer_set_ccr(12, 1200);
        // 🔥 使用 actuator_outputs 发布11/12/13/14路舵机信号 - 扩展PWM输出映射：
        // actuator_outputs_s outputs{};
        // outputs.timestamp = hrt_absolute_time();

        // 初始化所有输出通道为0
        // for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
        //     outputs.output[i] = 0.0f;
        // }

        // 设置输出通道数 - 支持14路PWM
        // outputs.noutputs = 14;

        // 将舵机输出从 [0,1] 映射到 [-1,1]（兼容PWM混合器格式）
        // 11号舵机 -> outputs.output[10] (PWM_MAIN_11)
        // outputs.output[10] = (servo_13_output * 2.0f) - 1.0f;

        // 12号舵机 -> outputs.output[11] (PWM_MAIN_12)
        // outputs.output[11] = (servo_14_output * 2.0f) - 1.0f;

        // 发布到 actuator_outputs，PWM驱动会处理这些信号
        // _outputs_pub.publish(outputs);
    }

    static int custom_command(int argc, char *argv[])
    {
        return print_usage("unknown command");
    }

    static int print_usage(const char *reason = nullptr)
    {
        if (reason) {
            PX4_WARN("%s", reason);
        }

    PRINT_MODULE_DESCRIPTION(
        R"DESCR_STR(
### Description
Heterogeneous 10PWM passthrough: maps manual stick inputs to actuator_motors directly.

Use with caution: bypasses attitude control and control allocator.
)DESCR_STR"
    );

        PRINT_MODULE_USAGE_NAME("hetero_passthrough", "controller");
        PRINT_MODULE_USAGE_COMMAND("start");
        PRINT_MODULE_USAGE_COMMAND("stop");
        PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

        return 0;
    }

private:
    uORB::Subscription _manual_sub{ORB_ID(manual_control_setpoint)};
    uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
    uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
    uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
    uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
    uORB::Publication<actuator_motors_s> _motors_pub{ORB_ID(actuator_motors)};
    uORB::Publication<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs)};

    // PID控制变量
    float speed_i{0.0f};
    float _last_yaw_speed_error{0.0f};
    float _yaw_integral{0.0f};
    float _last_yaw{0.0f};
    float _last_yaw_rate{0.0f};
    hrt_abstime _last_time{0};

        // 高度PID控制变量
        float _altitude_integral{0.0f};
        float _last_altitude{0.0f};
        float _last_altitude_error{0.0f};
        float _last_altitude_derivative{0.0f};
        float _target_altitude{0.0f};
        bool _altitude_hold_enabled{false};

        // 速度环控制变量
        float _velocity_integral{0.0f};
        float _target_velocity{0.0f};
        float _last_velocity_error{0.0f};

    // my_control参数变量
    float _my_control_yaw_kp{-0.2f};
    float _my_control_yaw_kd{-0.01f};
    float _my_control_yaw_ki{0.0f};
    float _my_control_yaw_integral_limit{0.2f};
    float _my_control_yaw_output_limit{0.99f};

    // Yaw速度环参数
    float _my_control_yaw_speed_kp{0.05f};
    float _my_control_yaw_speed_ki{0.0f};
    float _my_control_yaw_speed_integral_limit{0.2f};

    float _my_control_alt_kp_pos{-5.0f};
    float _my_control_alt_ki_pos{-0.05f};
    float _my_control_alt_kd_pos{-0.1f};
    float _my_control_alt_kp_vel{-0.5f};
    float _my_control_alt_ki_vel{0.0f};
    float _my_control_alt_kd_vel{0.0f};

    float _my_control_filter_alpha{0.3f};
    float _my_control_blend_rate{0.05f};

    // 水平位置控制参数
    float _my_control_pos_x_kp{0.5f};
    float _my_control_pos_x_ki{0.0f};
    float _my_control_pos_x_kd{0.1f};
    float _my_control_pos_x_ilim{0.5f};
    float _my_control_pos_x_olim{1.0f};

    float _my_control_pos_y_kp{10.5f};
    float _my_control_pos_y_ki{0.0f};
    float _my_control_pos_y_kd{0.1f};
    float _my_control_pos_y_ilim{0.5f};
    float _my_control_pos_y_olim{1.0f};

    // 水平速度控制参数
    float _my_control_vel_x_kp{0.8f};
    float _my_control_vel_x_ki{0.0f};
    float _my_control_vel_x_kd{0.0f};
    float _my_control_vel_x_ilim{0.5f};
    float _my_control_vel_x_olim{2.0f};

    float _my_control_vel_y_kp{0.8f};
    float _my_control_vel_y_ki{0.0f};
    float _my_control_vel_y_kd{0.0f};
    float _my_control_vel_y_ilim{0.5f};
    float _my_control_vel_y_olim{2.0f};


    // 位置保持参数
    float _my_control_pos_hold_deadzone{0.1f};

    // 水平位置控制状态变量
    float _target_position_x{0.0f};
    float _target_position_y{0.0f};
    float _pos_x_integral{0.0f};
    float _pos_y_integral{0.0f};
    float _target_velocity_x{0.0f};
    float _target_velocity_y{0.0f};
    float _vel_x_integral{0.0f};
    float _vel_y_integral{0.0f};

    double target_setpoint_yaw{0.0f};
    float yaw_rate_error{0.0f};

};

extern "C" __EXPORT int hetero_passthrough_main(int argc, char *argv[])
{
    return HeteroPassthrough::main(argc, argv);
}
